﻿#pragma once
namespace graph
{
   namespace math
   {
      inline lat_lon_t grad_to_rad(lat_lon_t fi)
      {
          lat_lon_t pi = 3.1415926535f;
          return pi*fi/180;
      }
      
      inline lat_lon_t rad_to_grad(lat_lon_t rad)
      {
          lat_lon_t pi = 3.1415926535f;
          return rad*180/pi;
      }
      
      inline lat_lon_t euclid_distance(lat_lon_t lat1, lat_lon_t lng1, lat_lon_t lat2, lat_lon_t lng2)
      {
          return sqrt( pow(lat1-lat2,2) + pow(lng1-lng2,2) );
      }
      inline lat_lon_t euclid_distance(point_t p1, point_t p2)
      {
          return euclid_distance(p1.lat_, p1.lon_, p2.lat_, p2.lon_);
      }
      
      inline lat_lon_t manhattan_distance(lat_lon_t lat1, lat_lon_t lng1, lat_lon_t lat2, lat_lon_t lng2)
      {
          return  abs(lat1 - lat2) + abs(lng1 - lng2);
      }
      
      inline lat_lon_t manhattan_distance(point_t p1, point_t p2)
      {
          return manhattan_distance(p1.lat_, p1.lon_, p2.lat_, p2.lon_);
      }
      //result in kilometeres
      inline lat_lon_t sphere_distance(lat_lon_t lat1, lat_lon_t lng1, lat_lon_t lat2, lat_lon_t lng2)
      {
          lat1 = grad_to_rad(lat1);
          lat2 = grad_to_rad(lat2);
          lng1 = grad_to_rad(lng1);
          lng2 = grad_to_rad(lng2);
          lat_lon_t d_lambda = lng2 - lng1;
          lat_lon_t d_fi = lat2 - lat1;
          ////high precision
          //lat_lon_t d_sigma = atan( 
          //    sqrt( pow(cos(lat2)*sin(d_lambda),2) + pow( cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(d_lambda),2 ) )
          //    / ( sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(d_lambda) )
          //    );
          lat_lon_t d_sigma = 2*asin( sqrt( pow(sin(d_fi/2),2) + cos(lat1)*cos(lat2)*pow(sin(d_lambda/2),2)) );

          return d_sigma * 6378.1370f;
          
          //low precision
          //return 6378.1370f * acos( cos(lat1) * cos(lat2) + sin(lat1) * sin(lat2) * cos(lng1 - lng2)); //usuall
      }
      
      // r = 6,378.1370
      // pi 3.1415926535 
      inline lat_lon_t distance(lat_lon_t lat1, lat_lon_t lng1, lat_lon_t lat2, lat_lon_t lng2)
      {
          return  sphere_distance(lat1,lng1,lat2,lng2);
      }
      
      inline lat_lon_t distance(point_t p1, point_t p2)
      {
          return  distance(p1.lat_, p1.lon_, p2.lat_, p2.lon_);
      }
      
      inline lat_lon_t distance(vertex_t const & v, point_t p2)
      {
          return  distance(v.get_point(), p2);
      }
      //TODO check it
      //only for plain!!! not circle
      inline point_t edge_middle_point(vertex_t const & source_v, vertex_t const & target_v)
      {         
          return point_t( 
               (lat_lon_t)(0.5*(source_v.get_point().lat_ + target_v.get_point().lat_)),
               (lat_lon_t)(0.5*(source_v.get_point().lon_ + target_v.get_point().lon_))
          );
      }      

      point_t lat_lon_to_xy(point_t p, lat_lon_t lon0);
      point_t xy_to_lat_lon(point_t p, lat_lon_t lon0);

      //works only for sphere
      //uses merkaator projection to correctly work with angles
      //@ returns distance
      inline lat_lon_t nearest_point(point_t point, point_t edge_source_p, 
                                     point_t edge_target_p, point_t& nearest_point, lat_lon_t& t)
      {        
         edge_target_p = lat_lon_to_xy(edge_target_p, 0);
         edge_source_p = lat_lon_to_xy(edge_source_p, 0);
         point = lat_lon_to_xy(point, 0);

         //for parametric representtion of line
         typedef std::pair<lat_lon_t, lat_lon_t> value; // = first + second*t
         value gen_x_of_line(0, 0); //general point of line
         gen_x_of_line.first = edge_source_p.lon_;
         gen_x_of_line.second = edge_target_p.lon_ - edge_source_p.lon_;

         value gen_y_of_line(0, 0); //general point of line
         gen_y_of_line.first = edge_source_p.lat_;
         gen_y_of_line.second = edge_target_p.lat_ - edge_source_p.lat_;

         point_t line_vect(edge_target_p.lat_ - edge_source_p.lat_, 
                           edge_target_p.lon_ - edge_source_p.lon_
                          );
         lat_lon_t b = 0; //for parameter t value, which gives nearest point
         lat_lon_t a = 0; //

         point_t przd_vect_to_point_xy, przd_vect_to_point_txy; //vector from nearest point to point 
         przd_vect_to_point_xy.lat_ = point.lat_ - gen_y_of_line.first;
         przd_vect_to_point_xy.lon_ = point.lon_ - gen_x_of_line.first;
         przd_vect_to_point_txy.lat_ = - gen_y_of_line.second;
         przd_vect_to_point_txy.lon_ = - gen_x_of_line.second;
         //scalar mul line vect and przd vect 
         a += line_vect.lat_ * przd_vect_to_point_xy.lat_;
         b += line_vect.lat_ * przd_vect_to_point_txy.lat_;
         a += line_vect.lon_ * przd_vect_to_point_xy.lon_;
         b += line_vect.lon_ * przd_vect_to_point_txy.lon_;
         //a + bt = 0
         t = -a / b;
         if (0 <= abs(t) && abs(t) <= 1)
         {
            nearest_point.lat_ = gen_y_of_line.first + gen_y_of_line.second * t;
            nearest_point.lon_ = gen_x_of_line.first + gen_x_of_line.second * t;
         }
         else if (t < 0)
            nearest_point = edge_source_p;
         else
            nearest_point = edge_target_p;
         nearest_point = xy_to_lat_lon(nearest_point, 0);
         return distance(point, nearest_point);
      }

      //XY are global positions, not screen positions
      //X,Y are in metres
      inline point_t lat_lon_to_xy(lat_lon_t lat, lat_lon_t lon, lat_lon_t lon0)
      {
          lat_lon_t C=1000;
          lat_lon_t pi = 3.1415926535f;
          point_t result;
          result.lat_ = C*grad_to_rad(lon - lon0);
          result.lon_ = C*log(tan(grad_to_rad(lat)/2 + pi/4)) - 1300;
          //printf("%f: %f | %f: %f \n",lat, result.second, lon, result.first);
          return result;
      }

      inline point_t lat_lon_to_xy(point_t p, lat_lon_t lon0)
      {
         return lat_lon_to_xy(p.lat_, p.lon_, lon0);
      }
      
      inline point_t xy_to_lat_lon(lat_lon_t x, lat_lon_t y, lat_lon_t lon0)
      {
          lat_lon_t C = 1000;
          lat_lon_t pi = 3.1415926535f;
          point_t result;
          lat_lon_t e = 2.71828f;
          result.lon_ = (rad_to_grad(x/C) + lon0); //lon
          result.lat_ = rad_to_grad( 2* atan(exp((y + 1300)/C)) - pi/2 ); //lat
          return result;
      }

      inline point_t xy_to_lat_lon(point_t p, lat_lon_t lon0)
      {
         return xy_to_lat_lon(p.lat_, p.lon_, lon0);
      }
      
      //> 0 if > 0
      //0 if == 0
      //< 0 if < 0
      inline int scalar_mult_sgn(point_t vect1, point_t vect2)
      {
         lat_lon_t scal_mult = vect1.lat_ * vect2.lat_ + vect1.lon_ * vect2.lon_;
         if (scal_mult > 0)
            return 1;
         else if (scal_mult < 0)
            return -1;
         else 
         //   return 0;
         {
            mpq_t lat1, lon1, lat2, lon2, result, zero;
            mpq_inits(lat1, lon1, lat2, lon2, result, zero, 0); //zero == 0
            mpq_set_d(lat1, vect1.lat_);
            mpq_set_d(lon1, vect1.lon_);
            mpq_set_d(lat2, vect2.lat_);
            mpq_set_d(lon2, vect2.lon_);
            mpq_mul(lat1, lat1, lat2);
            mpq_mul(lon1, lon1, lon2);
            mpq_add(result, lat1, lon1);
            
            int cmp_res = mpq_cmp(result, zero);
            mpq_clears(lat1, lon1, lat2, lon2, result, zero, 0);
            return cmp_res; //0 is equals, > 0 if > 0, < 0 if < 0
         }
      }
      
      //additional math at
      //http://www.movable-type.co.uk/scripts/latlong.html
   }
}
